#ifndef __SERVORATE_H__
#define __SERVORATE_H__ 1

#include "Arduino.h"

#include "../Servo/Servo.h"
#include "../pt/pt.h"

#define DELAY_SERVO_AIRTRONICS_94102 4500
#define DELAY_SERVO_CIRRUS_CS_10BB 2250

struct TsMicroDelay
{
  struct pt pt;
  unsigned long nMicrosStop;
};

struct TsServo
{
  struct pt pt;
  Servo obj;
  unsigned int nCurPosition;
  unsigned int nTargetPosition;
  unsigned long nLastMoveTimestampInMicros;
  unsigned long nMaxServoSpeedInMicrosPerDegree;
  unsigned int nMoveRateInDegPerSec;
  struct TsMicroDelay tsMicroDelay;
};

void servoInit( struct TsServo & tsServo, int nPin,
                unsigned long nMaxServoSpeedInMicrosPerDegree,
                unsigned int nInitialPosition,
                unsigned int nInitialRateInDegPerSec );

unsigned int servoGetPosition( struct TsServo & tsServo );

void servoSetPosition( struct TsServo & tsServo, int nTargetPosition,
                       int nMoveRateInDegPerSec );

boolean servoIsMoving( struct TsServo &tsServo );

PT_THREAD( threadMicroDelay( struct TsMicroDelay & tsMicroDelay, unsigned long nDelayMicros ) );

PT_THREAD( threadServoController( struct TsServo & tsServo ) );

#endif // __SERVORATE_H__
